#include "bsp_can.h"
#include "bsp_dwt.h"

static CANInstance* CAN_Instance_List[CAN_MAX_NUM] = {NULL};
static uint8_t Idx = 0;

static void CANServiceInit()
{
    CAN_FilterTypeDef can_filter_st;
    can_filter_st.FilterActivation = ENABLE;
    can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
    can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
    can_filter_st.FilterIdHigh = 0x0000;
    can_filter_st.FilterIdLow = 0x0000;
    can_filter_st.FilterMaskIdHigh = 0x0000;
    can_filter_st.FilterMaskIdLow = 0x0000;
    can_filter_st.FilterBank = 0;
    can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
    HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
    HAL_CAN_Start(&hcan1);
    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);


    can_filter_st.SlaveStartFilterBank = 14;
    can_filter_st.FilterBank = 14;
    HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
    HAL_CAN_Start(&hcan2);
    HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);
}

CANInstance* CANInit(CAN_Init_Config_s* _config) 
{
    CANInstance* instance = (CANInstance*)malloc(sizeof(CANInstance));
    memset(instance, 0, sizeof(CANInstance));

    instance -> tx_conf.StdId = _config -> tx_id;
    instance -> tx_conf.IDE = CAN_ID_STD;
    instance -> tx_conf.RTR = CAN_RTR_DATA;
    instance -> tx_conf.DLC = 8;

    instance -> CANx = _config -> CANx;
    instance -> tx_id = _config -> tx_id;
    instance -> rx_id = _config -> rx_id;
    instance -> can_module_callback = _config -> can_module_callback;
    instance -> id = _config -> id;
    CAN_Instance_List[Idx ++] = instance;
    return instance;
}

static void CANFIFOxCallback(CAN_HandleTypeDef* hcan, uint8_t fifox) 
{
    CAN_RxHeaderTypeDef rx_conf;
    uint8_t rx_buf[8];
    HAL_CAN_GetRxMessage(hcan, fifox, &rx_conf, rx_buf);
    for (uint8_t i = 0; i < Idx; i ++) 
    {
        if (hcan == CAN_Instance_List[i] -> CANx
         && rx_conf.StdId == CAN_Instance_List[i] -> rx_id
         && NULL != CAN_Instance_List[i] -> can_module_callback) 
        {
            CAN_Instance_List[i] -> rx_len = rx_conf.DLC;
            memcpy(CAN_Instance_List[i] -> rx_buf, rx_buf, rx_conf.DLC);
            CAN_Instance_List[i] -> can_module_callback(CAN_Instance_List[i]);
            return;
        }
    }
}

uint8_t CANSend(CANInstance* _instance, float timeout) 
{
    static uint32_t busy_count;
    static volatile float wait_time __attribute__((unused)); // for cancel warning
    float dwt_start = DWT_GetTimeline_ms();
    while (HAL_CAN_GetTxMailboxesFreeLevel(_instance-> CANx) == 0) // 等待邮箱空闲
    {
        if (DWT_GetTimeline_ms() - dwt_start > timeout) // 超时
        {
            busy_count++;
            return 0;
        }
    }
    wait_time = DWT_GetTimeline_ms() - dwt_start;
    // tx_mailbox会保存实际填入了这一帧消息的邮箱,但是知道是哪个邮箱发的似乎也没啥用
    if (HAL_CAN_AddTxMessage(_instance -> CANx, &_instance -> tx_conf, _instance -> tx_buf, &_instance -> tx_mailbox))
    {
        busy_count++;
        return 0;
    }
    return 1; // 发送成功
}

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan) 
{
    CANFIFOxCallback(hcan, CAN_RX_FIFO0);
}

void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef* hcan) 
{
    CANFIFOxCallback(hcan, CAN_RX_FIFO1);
}